#ifndef WALKAWARE_SHARED_POINTCLOUDEXTRACTOR_H
#define WALKAWARE_SHARED_POINTCLOUDEXTRACTOR_H

#include <math.h>

#include <WalkaWare/Shared/Mat4.h>
#include <WalkaWare/Shared/RawDepthMap.h>
#include <WalkaWare/Shared/PointCloud.h>
#include <WalkaWare/Kinect/CalibrationData.h>

namespace WalkaWare
{

namespace Shared
{

class PointCloudExtractor
{
	public:

		typedef void                BaseType;
		typedef PointCloudExtractor ThisType;

		PointCloudExtractor(void)
		{
			this->depthUnproject.SetIdentity();
		}

		virtual ~PointCloudExtractor(void)
		{
			;
		}

		bool loadCalibration(const char * fileName)
		{
			if (!this->calibration.load(fileName)) return false;

			Mat4f flip;
			flip.SetIdentity();
			flip[1][1] = -1.0f;
			flip[2][2] = -1.0f;

			const Mat3f & d = this->calibration.depthIntrinsics();

			Mat4f m;
			m.SetIdentity();
			for (int i=0; i<3; ++i) { for (int j=0; j<3; ++j) { m[i][j] = d[i][j]; } }

			this->depthUnproject = flip * vcg::Inverse(m);
			return true;
		}

		bool extract(const RawDepthMap & depthMap, PointCloud & pointCloud)
		{
			if (depthMap.isEmpty()) return false;

			const int w = depthMap.width();
			const int h = depthMap.height();

			pointCloud.clear();
			QVector<Vec3f> & points = pointCloud.points();
			points.reserve(w * h);

			for (int y=0; y<h; ++y)
			{
				for (int x=0; x<w; ++x)
				{
					const float z = ThisType::adjustDepth(depthMap(x, y));
					if (z == 0.0f) continue;

					const Vec3f point = this->depthUnproject * Vec3f(float(x)*z, float(y)*z, z);
					points.push_back(point);
				}
			}

			return true;
		}

	protected:

		Kinect::CalibrationData calibration;
		Mat4f                   depthUnproject;

		static float adjustDepth(quint16 rawDepth)
		{
			if (rawDepth >= 2047) return 0.0f;

			const float k1 = 1.1863f;
			const float k2 = 2842.5f;
			const float k3 = 0.1236f;

			const float d = k3 * float(tan(float(rawDepth) / k2 + k1));
			return d;
		}
};

} // end namespace Shared

} // end namespace WalkaWare

#endif // WALKAWARE_SHARED_POINTCLOUDEXTRACTOR_H
